#include "Quaternion.h"
#include <cmath>

namespace math
{
	void Quaternion::FromRotationMatrix( const Matrix& mRot )
	{
		// Algorithm in Ken Shoemake's article in 1987 
		// "Quaternion Calculus and Fast Animation"
		float fTrace = mRot.xx + mRot.yy + mRot.zz;
		if (fTrace > 0.0f)
		{
			// |w| > 1/2, may as well choose 2 > 1/2
			float fRoot = sqrtf(fTrace + 1.0f);
			w      = 0.5f * fRoot;
			fRoot  = 0.5f / fRoot; // 1/(4w)
			x      = (mRot.zy - mRot.yz) * fRoot;
			y      = (mRot.xz - mRot.zx) * fRoot;
			z      = (mRot.yx - mRot.xy) * fRoot;
		} 
		else
		{
			// |w| < 1/2
			static size_t nNext[3] = {1, 2, 0};
			size_t  i = 0;
			if(mRot.yy > mRot.xx)
				i = 1;
			if(mRot.zz > mRot.fM44[i][i])
				i = 2;
			size_t  j = nNext[i];
			size_t  k = nNext[j];

			float fRoot = sqrtf(mRot.fM44[i][i] - mRot.fM44[j][j] - mRot.fM44[k][k] + 1.0f);
			float *apkQuat[3] = {&x, &y, &z};
			*apkQuat[i] = 0.5f * fRoot;
			fRoot = 0.5f / fRoot;
			w = (mRot.fM44[j][k] - mRot.fM44[k][j]) * fRoot;
			*apkQuat[j] = (mRot.fM44[i][j] + mRot.fM44[j][i]) * fRoot;
			*apkQuat[k] = (mRot.fM44[i][k] + mRot.fM44[k][i]) * fRoot;
		}
	}

	void Quaternion::FromAngleAxis( float fAngle, const Vector3& vAxis )
	{
		// assert: axis[] is unit length
		// The quaternion representing the rotation is
		//   q = cos(A/2) + sin(A/2) * (x * i + y * j + z * k)
		
		if (fabsf(fAngle) < 1e-3)
		{
			w = 1.0f;
			x = y = z = 0.0f;
		}
		else
		{
			float fHaflAngle = fAngle * 0.5f;
			w = cosf(fAngle);

			float fSin = sinf(fAngle);
			x = fSin * vAxis.x;
			y = fSin * vAxis.y;
			z = fSin * vAxis.z;
		}
		
	}

	void Quaternion::FromAxes( const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis )
	{
		Matrix kRot;

		kRot.fM44[0][0] = xAxis.x;
		kRot.fM44[1][0] = xAxis.y;
		kRot.fM44[2][0] = xAxis.z;

		kRot.fM44[0][1] = yAxis.x;
		kRot.fM44[1][1] = yAxis.y;
		kRot.fM44[2][1] = yAxis.z;

		kRot.fM44[0][2] = zAxis.x;
		kRot.fM44[1][2] = zAxis.y;
		kRot.fM44[2][2] = zAxis.z;

		FromRotationMatrix(kRot);
	}

	Matrix Quaternion::ToRotationMatrix() const
	{
		Matrix kRot;

		float fTx  = x+x;
		float fTy  = y+y;
		float fTz  = z+z;
		float fTwx = fTx*w;
		float fTwy = fTy*w;
		float fTwz = fTz*w;
		float fTxx = fTx*x;
		float fTxy = fTy*x;
		float fTxz = fTz*x;
		float fTyy = fTy*y;
		float fTyz = fTz*y;
		float fTzz = fTz*z;
		
		kRot.fM44[0][0] = 1.0f-(fTyy+fTzz);
		kRot.fM44[0][1] = fTxy-fTwz;
		kRot.fM44[0][2] = fTxz+fTwy;
		kRot.fM44[1][0] = fTxy+fTwz;
		kRot.fM44[1][1] = 1.0f-(fTxx+fTzz);
		kRot.fM44[1][2] = fTyz-fTwx;
		kRot.fM44[2][0] = fTxz-fTwy;
		kRot.fM44[2][1] = fTyz+fTwx;
		kRot.fM44[2][2] = 1.0f-(fTxx+fTyy);

		return kRot;
	}

	void Quaternion::ToRotationMatrix( Matrix& kRot ) const
	{
		float fTx  = x+x;
		float fTy  = y+y;
		float fTz  = z+z;
		float fTwx = fTx*w;
		float fTwy = fTy*w;
		float fTwz = fTz*w;
		float fTxx = fTx*x;
		float fTxy = fTy*x;
		float fTxz = fTz*x;
		float fTyy = fTy*y;
		float fTyz = fTz*y;
		float fTzz = fTz*z;
		
		kRot.fM44[0][0] = 1.0f-(fTyy+fTzz);
		kRot.fM44[0][1] = fTxy-fTwz;
		kRot.fM44[0][2] = fTxz+fTwy;
		kRot.fM44[1][0] = fTxy+fTwz;
		kRot.fM44[1][1] = 1.0f-(fTxx+fTzz);
		kRot.fM44[1][2] = fTyz-fTwx;
		kRot.fM44[2][0] = fTxz-fTwy;
		kRot.fM44[2][1] = fTyz+fTwx;
		kRot.fM44[2][2] = 1.0f-(fTxx+fTyy);
	}

	void Quaternion::ToAngleAxis( float& fAngle, Vector3& vAxis ) const
	{
		float fSqrLength = x * x + y * y + z * z;
		if (fSqrLength > 0.0f)
		{
			fAngle = 2.0f * acosf(w);
			float fInvLength = 1 / sqrtf(fSqrLength);
			vAxis.x = x * fInvLength;
			vAxis.y = y * fInvLength;
			vAxis.z = z * fInvLength;
		}
		else
		{
			fAngle  = 0.0f;
			vAxis.x = 1.0f;
			vAxis.y = 0.0f;
			vAxis.z = 0.0f;
		}
	}

	void Quaternion::ToAxes( Vector3& xAxis, Vector3& yAxis, Vector3& zAxis )
	{
		Matrix kRot;

		ToRotationMatrix(kRot);

		xAxis.x = kRot.fM44[0][0];
		xAxis.y = kRot.fM44[1][0];
		xAxis.z = kRot.fM44[2][0];

		yAxis.x = kRot.fM44[0][1];
		yAxis.y = kRot.fM44[1][1];
		yAxis.z = kRot.fM44[2][1];

		zAxis.x = kRot.fM44[0][2];
		zAxis.y = kRot.fM44[1][2];
		zAxis.z = kRot.fM44[2][2];
	}

	Vector3 Quaternion::XAxis() const
	{
		float fTy  = 2.0f*y;
		float fTz  = 2.0f*z;
		float fTwy = fTy*w;
		float fTwz = fTz*w;
		float fTxy = fTy*x;
		float fTxz = fTz*x;
		float fTyy = fTy*y;
		float fTzz = fTz*z;
		return Vector3(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
	}

	Vector3 Quaternion::YAxis() const
	{
		float fTx  = 2.0f*x;
		float fTy  = 2.0f*y;
		float fTz  = 2.0f*z;
		float fTwx = fTx*w;
		float fTwz = fTz*w;
		float fTxx = fTx*x;
		float fTxy = fTy*x;
		float fTyz = fTz*y;
		float fTzz = fTz*z;
		return Vector3(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
	}

	Vector3 Quaternion::ZAxis() const
	{
		float fTx  = 2.0f*x;
		float fTy  = 2.0f*y;
		float fTz  = 2.0f*z;
		float fTwx = fTx*w;
		float fTwy = fTy*w;
		float fTxx = fTx*x;
		float fTxz = fTz*x;
		float fTyy = fTy*y;
		float fTyz = fTz*y;
		return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
	}

	Quaternion Quaternion::operator+( const Quaternion& rQt ) const
	{
		return Quaternion(w + rQt.w, x + rQt.x, y + rQt.y, z + rQt.z);
	}

	Quaternion Quaternion::operator-( const Quaternion& rQt ) const
	{
		return Quaternion(w - rQt.w, x - rQt.x, y - rQt.y, z - rQt.z);
	}

	Quaternion Quaternion::operator*( const Quaternion& rkQ ) const
	{
		return Quaternion
			(
			w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
			w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
			w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
			w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
			);
	}

	Quaternion Quaternion::operator*( float fScalar ) const
	{
		return Quaternion(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
	}

	Vector3 Quaternion::operator*( const Vector3& rkVector ) const
	{
		Vector3 uv, uuv;
		Vector3 qvec(x, y, z);
		uv  = CrossProduct(qvec, rkVector);
		uuv = CrossProduct(qvec, uv);
		uv  *= (2.0f * w);
		uuv *= 2.0f;

		return rkVector + uv + uuv;
	}

	Quaternion Quaternion::operator-() const
	{
		return Quaternion(-w,-x,-y,-z);
	}

	float Quaternion::DotProduct( const Quaternion& rkQ ) const
	{
		return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
	}

	float Quaternion::Length()
	{
		return sqrtf(w*w+x*x+y*y+z*z);
	}

	void Quaternion::Normalize()
	{
		float len = Length();
		float factor = 1.0f / len;
		*this = *this * factor;
	}

	Quaternion Quaternion::Inverse() const
	{
		float fNorm = w*w+x*x+y*y+z*z;
		if ( fNorm > 0.0 )
		{
			float fInvNorm = 1.0f / fNorm;
			return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
		}
		else
		{
			// return an invalid result to flag the error
			return Quaternion(0.0f, 0.0f, 0.0f, 0.0f);
		}
	}

	Quaternion Quaternion::Slerp( const Quaternion& p, const Quaternion& q, float t )
	{
		float fCos = p.DotProduct(q);
		Quaternion qResult = q;

		if (fabsf(fCos) < 1 - 1e-3)
		{
			float fSin = sqrtf(1 - fCos * fCos);
			float fAngle = atan2(fSin, fCos);
			float fInvSin = 1.0f / fSin;
			float fCoeff0 = sinf((1.0f - t) * fAngle) * fInvSin;
			float fCoeff1 = sinf(t * fAngle) * fInvSin;
			return p * fCoeff0 + q * fCoeff1;
		}
		else
		{
			Quaternion tmp = p * (1.0f - t) + q * t;
			tmp.Normalize();
			return tmp;
		}
	}

	Quaternion Quaternion::Conjugate( const Quaternion& q )
	{
		return Quaternion(q.w, -q.x, -q.y, -q.z);
	}

	Quaternion Quaternion::Power( const Quaternion& q, float fExponent )
	{
		if (fabsf(q.w) > (1 - 1e-3))
		{
			return q;
		}
		
		float fAngle = acosf(q.w);
		float fNewAngle = fAngle * fExponent;

		Quaternion result;
		result.w = cosf(fNewAngle);

		float fCoeff = sinf(fNewAngle) / sinf(fAngle);
		result.x = fCoeff * q.x;
		result.y = fCoeff * q.y;
		result.z = fCoeff * q.z;
		
		return result;
	}

}